#include "DashboardClient.h"

const char* DashboardClient::dashIP = "10.3.8.5";

DashboardClient::DashboardClient() {
	dashSocket = -1;
	P = 0;
	I = 0;
	D = 0;
	n = 1;
	angleRequested = false;
	dataAngle = -1;
}

void DashboardClient::ConnectToDash() {
	struct sockaddr_in sa;
	struct hostent *hp;
	int s;
	printf("Declared structs\n");
	if ((hp = gethostbyname(dashIP)) == NULL) {
		errno = ECONNREFUSED;
		printf("DashClient CallServer failed\n");
		return;
	}
	printf("Got host\n");
	memset(&sa, 0, sizeof(sa));
	printf("Copied socketaddr\n");
	memcpy((char*) &sa.sin_addr, hp->h_addr, hp->h_length); /*set address */
	printf("Set addr of host\n");
	sa.sin_family = hp->h_addrtype;
	printf("Set sockaddr family type\n");
	sa.sin_port = dashPort;
	printf("Set port\n");
	if ((s = socket(hp->h_addrtype, SOCK_STREAM, 0)) < 0) {
		printf("DashClient CallServer failed\n");
		return;
	}

	printf("Created socket\n");
	if (connect(s, (struct sockaddr*) &sa, sizeof(sa)) < 0) {
		close(s);
		printf("DashClient CallServer failed\n");
		return;
	}
	int nonblocking = 1;
	if (ioctl(s, FIONBIO, (int)(&nonblocking)) < 0) 
	{ 
		printf("Well it didn't work :'C\n"); 
	}
	printf("Connected!\n");
	dashSocket = (s);


	printf("Connected with socket ID: %d\n", dashSocket);
}

void DashboardClient::PeriodicUpdate() {
	if (dashSocket > 0) {
		int size = read(dashSocket, tempbuff, BUFFER_SIZE);
		if (size > 0) {
			printf("Got %d bytes\n", size);
			std::string data(tempbuff, tempbuff + size);
			buffer.append(data);
			std::size_t EOTPos = buffer.find_first_of(hEOT);
			if (EOTPos == std::string::npos)
			{
				printf("Could not find EOT in:\n");
				for (unsigned int i=1;i<buffer.length();i++)
				{
					printf("%d ", buffer[i]);
				}
				printf("\n");
			}
			if (EOTPos != std::string::npos) {
				BlockReceived(EOTPos);
			}
		}
	}
}

void DashboardClient::SetNWInt(int index, int n)
{
	nwInt[index] = n;
//	nwbuffer
}

void DashboardClient::SetNWFloat(int index, float n)
{
//	nwFloat[index] = n;
//	size_t needed = snprintf(NULL, 0, "%c%c%d %f", 'n','i',index,n);
//	nwbuffer = malloc(needed);
//	sprintf(nwbuffer,"%c%c%d %f", 'n','i',index,n);)
//	write(dashSocket, (char*)nwbuffer, needed);
}

void DashboardClient::DataReceived(char header, std::string data) {
	converter.clear();
	printf("Got data:\n");
	switch (header) {
	case 'a':
		converter << data;
		//float fAngle;
		//std::cout<<data;
		converter >> dataAngle;
		printf("Angle: %.4f\n", dataAngle);
		//DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line3,"Angle:%f",dataAngle);
		break;
	case 'p':
			converter << data;
			converter >> P;
			CommandBase::shooterAngle->SetPID(P,I,D);
			break;
	case 'i':
			converter << data;
			converter >> I;
			CommandBase::shooterAngle->SetPID(P,I,D);
			break;
	case 'd':
			converter << data;
			converter >> D;
			CommandBase::shooterAngle->SetPID(P,I,D);
			break;
			
	case 'n':
			char subheader;
			int index;
			converter >> subheader;
			converter >> index;
			switch(subheader) {
			case 'i':
				converter >> nwInt[index];
				break;
			case 'f':
				converter >> nwFloat[index];
				break;
			}
	default:
		printf("\tHeader: %c\nData: ", header);
		for (unsigned int i=0;i<data.length();i++)
		{
			printf("%c", data[i]);
		}
		printf("\n");
		break;
	}
}

void DashboardClient::BlockReceived(size_t pos) {
	printf("Block recieved");
	std::string data(buffer);
	buffer = data.substr(pos);
	buffer.erase(buffer.begin());
	data.resize(pos);
	char header = data[0];
	data.erase(data.begin());
	DataReceived(header, data);
	pos = buffer.find_first_of(hEOT);

	if (pos != std::string::npos) {
		BlockReceived(pos);
	}
}

int DashboardClient::GetNWInt(int index)
{
	if (index < 0 || index > 255) {
			printf("In GetNWInt - index (%d) out of range.",index);
			return 0;
	}
	return nwInt[index];
}

float DashboardClient::GetNWFloat(int index)
{
	if (index < 0 || index > 255) {
		printf("In GetNWFloat - index (%d) out of range.",index);
		return 0.0;
	}
	return nwFloat[index];
}
float DashboardClient::GetAngle() {
	return dataAngle;
}

float DashboardClient::GetP()
{
	return P;
}

float DashboardClient::GetI()
{
	return I;
}

float DashboardClient::GetD()
{
	return D;
}
